#include "../include/humandetector_vis/visualizer.h"
using namespace Human_Visualizer;
HumanVisualizer::HumanVisualizer():nh()
{

}

bool HumanVisualizer::init()
{
    try
    {

        nh.param(topic_str,topic,topic_default);
        nh.param(frame_str,frame,frame_default);
        humenSubscriber= nh.subscribe<hri_msgs::Humen>(topic,100,
                                                       (boost::function<void(const hri_msgs::HumenConstPtr&)>)
                                                       boost::bind(&HumanVisualizer::humenSubscriberCB, this, _1));

        humen_marker_publisher=nh.advertise<visualization_msgs::MarkerArray>("/humen_marker",1);

        //    transform_listener= new tf::TransformListener(ros::Duration(1000));
    }
    catch(...)
    {
        return false;
    }
    return true;

}

int HumanVisualizer::run()
{
    //    ros::Rate loop_rate(100);
    //    while(ros::ok())
    //    {
    //        ros::spinOnce();
    //        loop_rate.sleep();
    //    }
    setColors();
    ros::spin();
    return 0;

}

void HumanVisualizer::humenSubscriberCB(const hri_msgs::HumenConstPtr& humen)
{

    visualization_msgs::MarkerArray marker_array;
    ROS_INFO("publishing");
    for (uint i = 0; i < humen->humen.size(); i++)
    {
        marker_array.markers.push_back(createMarker(humen->humen[i]));
    }
    humen_marker_publisher.publish(marker_array);
}

void HumanVisualizer::setColors()
{
    colors[9].r=1;
    colors[9].g=0;
    colors[9].b=0;
    colors[9].a=1;

    colors[1].r=0;
    colors[1].g=1;
    colors[1].b=0;
    colors[1].a=1;

    colors[2].r=0;
    colors[2].g=0;
    colors[2].b=1;
    colors[2].a=1;

    colors[3].r=1;
    colors[3].g=0;
    colors[3].b=1;
    colors[3].a=1;



    colors[4].r=1;
    colors[4].b=1;
    colors[4].g=0;
    colors[4].a=1;

    colors[5].b=1;
    colors[5].g=1;
    colors[5].r=0;
    colors[5].a=1;

    colors[6].b=1;
    colors[6].g=0.5;
    colors[6].r=0.5;
    colors[6].a=1;

    colors[7].b=0.5;
    colors[7].g=0;
    colors[7].r=0.5;
    colors[7].a=1;


    colors[8].b=1;
    colors[8].g=0.25;
    colors[8].r=0;
    colors[8].a=1;


}

geometry_msgs::PoseStamped HumanVisualizer::transform(geometry_msgs::Pose pose)
{
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.pose=pose;
    pose_stamped.pose.orientation.w = 1;

    pose_stamped.header.frame_id="/";
    pose_stamped.header.stamp=ros::Time::now();

    return pose_stamped;
}

visualization_msgs::Marker HumanVisualizer::createMarker(hri_msgs::Human human)
{
    visualization_msgs::Marker marker;
    marker.pose=human.position/*transform(human.position).pose*/;
    std::cout<<human.position;
    marker.header.frame_id = frame;
    std::cout<<frame<<endl;
    marker.header.stamp = ros::Time::now();

    marker.ns = "detected_human";
    marker.id = human.id.data;

    marker.type = visualization_msgs::Marker::CYLINDER;
    marker.action = visualization_msgs::Marker::ADD;

    marker.pose.orientation.w = 1;

//    marker.color.r = 1.0f;
//    marker.color.g = 0.0f;
//    marker.color.b = 0.0f;
//    marker.color.a = 1.0f;
    marker.color=colors[human.id.data];

    marker.scale.x = 0.5;
    marker.scale.y = 0.5;
    marker.scale.z = 0.5;

    marker.text = "Human";
    marker.lifetime = ros::Duration();
    return marker;
}
